#include "bsp_i2c.h"
#include "esp_log.h"
#include <iostream>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

static const char *TAG = "bsp_i2c";

extern "C" {

esp_err_t bsp_i2c_init(i2c_master_bus_handle_t *bus_handle, i2c_master_dev_handle_t *dev_handle)
{
    i2c_master_bus_config_t bus_config = {};
    bus_config.i2c_port = I2C_MASTER_NUM;
    bus_config.sda_io_num = (gpio_num_t)I2C_MASTER_SDA_IO;
    bus_config.scl_io_num = (gpio_num_t)I2C_MASTER_SCL_IO;
    bus_config.clk_source = I2C_CLK_SRC_DEFAULT;
    bus_config.glitch_ignore_cnt = 7;
    bus_config.flags.enable_internal_pullup = true;

    esp_err_t ret = i2c_new_master_bus(&bus_config, bus_handle);
    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG, "Failed to initialize I2C bus");
        return ret;
    }

    ESP_LOGI(TAG, "I2C initialized");
    return ESP_OK;
}

esp_err_t bsp_i2c_add_device(i2c_master_bus_handle_t bus_handle, i2c_master_dev_handle_t *dev_handle, uint8_t addr)
{
    i2c_device_config_t dev_config = {};
    dev_config.dev_addr_length = I2C_ADDR_BIT_LEN_7;
    dev_config.device_address  = addr;
    dev_config.scl_speed_hz    = I2C_MASTER_FREQ_HZ;
    dev_config.scl_wait_us     = 0;

    esp_err_t ret = i2c_master_bus_add_device(bus_handle, &dev_config, dev_handle);
    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG, "Failed to add I2C device");
        return ret;
    }

    ESP_LOGI(TAG, "I2C device added");
    return ESP_OK;
}

} // extern "C"
